/*
 * File: ControlService.java
 * 
 * Change Log
 * Author				DTG				Description of Change
 * ---------------------------------------------------------------------------
 * Timothy M. Lemm		18OCT11 0729	Initial Service Structure
 * 										Added binding to Core Service
 * 										Accepting Binds Support added 	
 * 
 * Timothy M. Lemm		19OCT11 1731	initial public methods added	
 * 
 * Timothy M. Lemm		19OCT11 2016	Compass and Gyro routines added
 */

package com.erau.droidsatframework.control;

import com.erau.droidsatframework.DroidSatActivity;
import com.erau.droidsatframework.core.CoreService;
import com.erau.droidsatframework.core.CoreService.CoreBinder;
import com.erau.droidsatframework.phidget.PhidgetService;
import com.erau.droidsatframework.phidget.PhidgetService.PhidgetBinder;

import android.app.Service;
import android.content.ComponentName;
import android.content.Context;
import android.content.Intent;
import android.content.ServiceConnection;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Binder;
import android.os.IBinder;
import android.util.Log;
import android.widget.Toast;

public class ControlService extends Service {

	private SensorManager adcSensorManagement;
	private Sensor adcGyroSensor;
	private Sensor adcCompassSensor;
	 
	private final IBinder localBinder = new ControlBinder(); //For dealing with incoming bind from control algo
	CoreService coreB;					//Local handle of the CoreService Instance for binding
	PhidgetService phidgetB;
	Algorithm alg;
	private static final String TAG = "Sensor  :";
	float xk_prev=0.0f;
    float Pk_prev=1.0f;
    float xk=0;
    final double noise=.006;
	private Intent control2PhidgetIntent;
	
	public boolean coreIsBound = false;	//Status indicator of being bound or not to the core
	public boolean phidgetIsBound = false;
	
	private float[] gyroRates = new float[3];
	private float[] gyroAngles = new float[3];
	private float[] compassPositions = new float[3];
	private float lastTimestamp = 0;
	private float compassReference;
	private int[] algRtrn = new int[3];
	public double tAngle = 0.0;
	private float filteredValue = 0.0f;
	private long eTime = 0;

	
	@Override
	public IBinder onBind(Intent intent) {
		return localBinder;
	}
	
	//CLASS: ControlBinderprivate static final String TAG = "Algorithm:";
	//Class that contains the information for the CoreService... 
	//We have a method so that multiple things can bind into us...
	public class ControlBinder extends Binder {
		public ControlService getService() {
			return ControlService.this;
		}//End getService
	}//End ControlBincer
	 
	@Override
	public void onCreate() {
		Toast.makeText(this, "Control Service is created", 2000).show();
		//Bind back to core //
		Intent control2CoreIntent = new Intent(this, CoreService.class);  //Intent to bind to core
		bindService(control2CoreIntent, bindConnection, Context.BIND_AUTO_CREATE ); //Bind
		
		control2PhidgetIntent = new Intent(this, PhidgetService.class);
		bindService(control2PhidgetIntent, bindConnection, Context.BIND_AUTO_CREATE);
		
		alg = new Algorithm();
		
		
		//Setup the sensors
		adcSensorManagement = (SensorManager) getSystemService(SENSOR_SERVICE);
		adcGyroSensor = adcSensorManagement.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
		adcCompassSensor = adcSensorManagement.getDefaultSensor(Sensor.TYPE_ORIENTATION);
		adcSensorManagement.registerListener(adcSensorListener, adcGyroSensor, SensorManager.SENSOR_DELAY_GAME);
		adcSensorManagement.registerListener(adcSensorListener, adcCompassSensor, SensorManager.SENSOR_DELAY_GAME);
	}
	
	@Override
	public void onDestroy(){
		//Toast.makeText(this, "Control is dead....", 2000).show(); 
		Log.v("Control", "Control Destroy Called");
		phidgetIsBound = false;
		stopService(control2PhidgetIntent);
		
	}
	
	@Override
	public int onStartCommand(Intent intent, int flags, int startId) {
		return START_NOT_STICKY; //We can die
	}//End onStartCommand
	
	//CLASS EXTENSION: bindConnection
	//Required class extension of Service Connection to provide a call 
	//back for the system to pass the bound interface back
	private ServiceConnection bindConnection = new ServiceConnection() {
		private static final String TAG = "Algorithm:";
		@Override
		public void onServiceConnected(ComponentName name, IBinder service) {
			String sName = name.getClassName().toString();
			
			if(name.getClassName().equals("com.erau.droidsatframework.core.CoreService")){
				CoreBinder coreBinder = (CoreBinder) service; //cast the call back service into the core binder
				coreB = coreBinder.getService();	//bring in the active instance of the core service
				coreIsBound = true;		//update status flag
				
				
			}
			else if(name.getClassName().equals("com.erau.droidsatframework.phidget.PhidgetService")){
				PhidgetBinder phidgetBinder = (PhidgetBinder) service;
				phidgetB = phidgetBinder.getService();
				phidgetIsBound = true;
			}
		}

		@Override
		public void onServiceDisconnected(ComponentName name) {
			if(name.getPackageName().equals("com.erau.droidsatframework.core.CoreService")){
				coreIsBound = false;
			}
			else if(name.getPackageName().equals("com.erau.droidsatframework.phidget.PhidgetService")){
				phidgetIsBound = false;
			}
		}
	};
	
	SensorEventListener adcSensorListener = new SensorEventListener() {
		
		@Override
		public void onSensorChanged(SensorEvent event) {
				Sensor eventSensor = event.sensor;////
			
				if(eventSensor.equals(adcGyroSensor)){
					
					if (lastTimestamp != 0) {
			              float deltaTime = (event.timestamp - lastTimestamp) * CONTROL_CONSTANTS.NANO_SEC_TO_SEC;
			              
			              //Hand off the anglular rates
			              gyroRates = event.values;
			              
			              filteredValue = kalman(gyroRates[2]);
			              //Convert and hand off change in angular position
			              //gyroAngles[0] = event.values[0]*deltaTime;//
			              //gyroAngles[1] = event.values[1]*deltaTime;
			              gyroAngles[2] = filteredValue*deltaTime;
			              //Log.v(TAG, "Raw Rate: "+Float.toString(gyroRates[2])+"  Filtered Rate: "+ Float.toString(filteredValue) +"  dAngle: "+Float.toString(gyroAngles[2]));
			          }
			          lastTimestamp = event.timestamp; //Update for deltaTime
			          eTime = event.timestamp;
			          
			          
			          algRtrn = alg.algorithm(gyroAngles[2], gyroRates[2]);
			          commandRequest(algRtrn[0], algRtrn[1], algRtrn[2]);
			          new Thread(new Runnable() {
			        	    public void run() {
			        	    	coreB.dal.logADC(eTime, filteredValue, alg.getTotalAngle(), algRtrn[0]);
			        	    }
			        	  }).start();
			          
			       }//End if gyro
				
				if(eventSensor.equals(adcCompassSensor)){
					compassPositions = event.values;
				}//End if orientation
				
				
		}//sensor change
		
		@Override
		public void onAccuracyChanged(Sensor sensor, int accuracy) {
			//lol we dont care...
		}			
		
	};
	
	//These are all deprecated methods, not used in the current code structure
	
	public float getGyroAngleX(){
		return gyroAngles[0];
	}//End getGryroX
	
	public float getGyroAngleY(){
		return gyroAngles[1];
	}//End getGyroY
	
	public float getGyroAngleZ(){
		return gyroAngles[2];
	}//End getGyroZ
	
	public float[] getGyroAngles(){
		return gyroAngles;
	}//End getGyroAngles
	
	public float getGyroRateX(){
		return gyroRates[0];
	}//End getGryroX
	
	public float getGyroRateY(){
		return gyroRates[1];
	}//End getGyroY
	
	public float getGyroRateZ(){
		return gyroRates[2];
	}//End getGyroZ
	
	public float[] getGyroRates(){
		return gyroRates;
	}//End getGyroRates
	
	public float getCompassPositionX(){
		return compassPositions[0];
	}//End getCompassPosX
	
	public float getCompassPositionY(){
		return compassPositions[1];	
	}//End getCompassPosY

	public float getCompassPositionZ(){
		return compassPositions[2];
	}//End getCompassPosZ
	
	public float[] getCompassPositions(){
		return compassPositions;
	}//End getCompassPositions
	
	public float getCompassReference(){
		return compassReference;
	}//End getCompassReference
	//End Deprecated methods
	
	//METHOD: commandRequest
	//Velocity has an acceptable range from -1000 to 1000
	//Acceleration has an acceptable range from 1-1000
	//Axis use the values in CONTROL_CONSTANTS.java
	public void commandRequest(int velocity, int acceleration, int axis){
		if(phidgetIsBound){
			phidgetB.commandProcess(velocity, acceleration, axis);
		}
		else{
			Toast.makeText(this, "apparently not bound...", 6000).show();
		}
	}//End Command Request
	
	private void log(long timestamp, double dAngle, double angle, double velocity){
		
	}
	
	public float kalman(float data) {

		final float A=1.0f;
		final float H=1.0f;
		final float Q=0.0f;
		final float R=.006f;
		float xk_prime;
		float Pk_prime;
		float Kk;
		float Pk;

		if (Math.abs(data)<noise) {
		xk_prime=A*xk_prev;
		Pk_prime=A*A*Pk_prev+Q;
		Kk=Pk_prime*H/(H*H*Pk_prime+R);
		xk=xk_prime*Kk*(data-H*xk_prime);
		Pk=(1-Kk*H)*Pk_prime;
		xk_prev=xk;
		Pk_prev=Pk;
		return xk;
		}
		else {
		return data;
		}


		}


}
